#include "robotFunctions.h"
using namespace std;
void robotWrite(string input, int b, map<int, swarmSocketClient> &clients, map<int, bool> &cons)
{
	if(b != 0)
	{
		//cout<< "something\n";
		if(cons[b])
		{
			cout<< clients[b].writeMessage(input.c_str(), input.size());
		}
		//cout<< "something\n";
	}
	else
	{
		for(int i = 1; i <= clients.size(); i++)
		{
			if(cons[i])
			{
				cout<< clients[i].writeMessage(input.c_str(), input.size());
			}
		} 
	}
}
int robotConnect(map<int, swarmSocketClient>& clients, map<int, string>& ips, map<int, string>& passes,
		map<int, bool>& cons, string& robotaddress, int botnum)
{
	ifstream fin;
	int b, i = 1;
	string input;
	fin.open(robotaddress.c_str());
	if(fin.fail())
	{
		return -1;
	}
	while(!fin.eof())
	{
		//perror("loop");
		fin >> input;
		if(fin.eof())
		{
			break;
		}
		ips[i] = input;
		ips[i] = ips[i].substr(ips[i].find("@")+1, ips[i].size() - ips[i].find("@")-1);
		fin >> passes[i];
		if(i == botnum)
		{
			clients[i];
			cons[i] = false;
			i++;
			continue;
		}
		//perror("client");
		clients.insert(pair<int, swarmSocketClient>(i, swarmSocketClient(ips[i].c_str(), 5555)));
		b = clients[i].initclient();		
		cons[i] = (0 == b);
		i++;
	}
	fin.close();
	return 0;
}

bool robotReconnect(map<int, swarmSocketClient>& clients, map<int, string>& ips, map<int, string>& passes,
		map<int, bool>& cons, int botnum)
{
	bool update = false;
	int b;
	for (map<int, string>::iterator it=ips.begin(); it!=ips.end(); ++it)
	{
		if(!cons[it->first] && it->first != botnum)
		{
			clients[it->first].shut();
			b = clients[it->first].initclient(it->second.c_str(), 5555);
			cons[it->first] = (0 == b);
			update = (0 == b) || update;
		}
		else if(it->first != botnum)
		{
			cons[it->first] = clients[it->first].checkConnection();
			update = (! cons[it->first]) || update;
		}
	}
	return update;
}

void textInterface(map<int, swarmSocketClient>& clients, map<int, string>& ips, map<int, string>& passes,
		map<int, bool>& cons, string& robotaddress)
{
	int b;
	char c;
	string input;
	system("clear");
	while(1)
	{
		
		printf("Select robot or esc to quit\n");
		printf("robot 0 is all %i\n", (int) ips.size());
		for(int i = 1; i <= ips.size(); i++)
		{
			printf("robot %i is %s connected:%i\n", i, ips[i].c_str(), cons[i]);
		}
		cin >> input;
		if(!(atoi(input.c_str())==0 && input[0] != '0'))
		{
			b = atoi(input.c_str());
			if(b > ips.size() || b < 0)
			{
				cout << "invalid robot: please try again";
			}
			else
			{
				system("clear");
				input = "";
				c = '\n';
				while(c != 'c')
				{
					printf("Selected robot is %i\n", b);
					printf("Select command\n");
					printf("R red light on\t\tr red light off\n");
					printf("G green light on\tg green light off\n");
					printf("B blue light on\t\tb blue light off\n");
					printf("< for left\t\t> for right\n");
					printf("( counter-clockwise\t) clockwise\n");
					printf("{ left go\t\t } right go\n");
					printf("[ left stop\t\t] right stop\n");
					printf("^ for foward\t\t_ for stop\n");
					printf("s for bump sensor data\n");
					printf("c for cancel\n");
					cin >> c;
					switch(c)
					{
						case 'R':
							input = "17";
							break;
						case 'r':
							input = "16";
							break;
						case 'G':
							input = "33";
							break;
						case 'g':
							input = "32";
							break;
						case 'B':
							input = "49";
							break;
						case 'b':
							input = "48";
							break;
						case '{':
							input = "129";
							break;
						case '[':
							input = "128";
							break;
						case '}':
							input = "65";
							break;
						case ']':
							input = "64";
							break;
						case '^':
							input = "193";
							break;
						case '_':
							input = "192";
							break;
						case '(':
							input = "197";
							break;
						case ')':
							input = "198";
							break;
						case '<':
						case '>':
							cout<< "input a number\n";
							cin >> input;
							
						break;
						case 'c':
							input = "c";
							break;
						default:
							system("clear");
							cout<< "invalid command: please try again\n";
							break;
					
					}
					if(input != "")
					{
						
						system("clear");
						input = c + input;
						robotWrite(input, b, clients, cons);
					}	
				}
			}
				
		}
		else
		{
			if(input[0] == 27)
			{
				cout<< "good bye\n";
				return ;
			}
			else
			{
				cout<< "invalid command: please try again\n";
			} 
		}
		
	}

}

string itoa(int i)
{
	char ch[500];
	sprintf(ch, "%i", i);
	string s = ch;
	return s;
}
string ftoa(double f)
{
	char ch[500];
	sprintf(ch, "%f", f);
	string s = ch;
	return s;
}

// Make all uppercase
string toupper(string s)
{
    string retVal(s);
    for (int i = 0; i < retVal.length(); i++){
        retVal[i] = toupper(retVal[i]);               
    }

    return retVal;
}
